#include "pid.h"
#include <math.h>



void pid_calculate(void *arg)
{
	pid_t *self = (pid_t*)arg;
	
	self->error = self->reference - self->feedback;

	if( self->integral_deathzoom != 0 )
	{
		if( fabs(self->error) <= fabs(self->integral_deathzoom) )
		{
			self->integral += self->error;
			if( fabs(self->integral) >= self->integral_limit )
			{
				self->integral = self->integral >= 0? self->integral_limit : -self->integral_limit;
			}
		}	
	}
	else 
	{
		self->integral += self->error;
		if( fabs(self->integral) >= fabs( self->integral_limit ) )
		{
			self->integral = self->integral >= 0? self->integral_limit : -self->integral_limit;
		}
	}

	self->output = self->kp * self->error			
						+ self->ki * self->integral * self->period
						+ self->kd * (self->error - self->last_error) / self->period;

	self->output = self->output >= self->output_limit ? self->output_limit : self->output <= -self->output_limit ? -self->output_limit:self->output;

	
	self->last_error = self->error;

}


